% calculate the mean data for Robotic Model
% use human mean angle data
% use robot parameters
addpath('dataTorso/');
addpath('Func/');
addpath('models/AMBER/build_torso')
% load human mean angles
load('mean_data_sankle.mat');
load('mean_data_sknee.mat');
load('mean_data_theta3.mat');
load('mean_data_theta4.mat');
load('mean_data_nsknee.mat');
time_len = length(ns_time);
addpath('/home/shu/Documents/Research_Related/version2_oldfittingfunction')
% reshape data
q = [s_ankle;s_knee';theta3';theta4';ns_knee'];
%%
% % testdotq = diff(q,1,2)/(ns_time(2)-ns_time(1));
%
    FWIN = 'hamming';
    F1 = 10;
    F2 = 175;
    F3 = 10;
    F4 = 20;
    F5 = 30;
    F6 = 20;
    for index = 1:5
sm_q(index,:)  = ndnanfilter(q(index,:),FWIN,F1);
dotq(index,:) = diff(sm_q(index,:),1,2)/(ns_time(2)-ns_time(1));
%
sm_dotq(index,:)  = ndnanfilter(dotq(index,:),FWIN,F1);
ddotq(index,:) = diff(sm_dotq(index,:),1,2)/(ns_time(2)-ns_time(1));
sm_ddotq(index,:)  = ndnanfilter(ddotq(index,:),FWIN,F1);
 plot(sm_q(index,:))
hold on;plot(q(index,:),'r')
    end
%% 
addpath('/home/shu/Documents/Research_Related/K2_NAO_under_Rev 2.12_nsslope_linearized ames/build_torso')
for index = 1:98
xzmp(index)=zmp([sm_q(:,index);sm_dotq(:,index);sm_ddotq(:,index)])
end
plot(xzmp)
%%
% COM for NAO
for i = 1:time_len
    NAO_com(i) = NAO_pcom_sca(q(:,i),  0, 0);
    NAO_delta_com(i) = NAO_deltapcom_sca(q(:,i),0,0);
end

COM = NAO_com';
save('dataTorso/mean_data_com.mat','COM','ns_time');
deltaCOM = NAO_delta_com';
save('dataTorso/mean_data_deltacom.mat','deltaCOM','ns_time');

plot(NAO_com,'r')
legend('COM','deltaCOM')
xlabel('time');
ylabel('Forward Direction');